/*! \file dynlib.h

\dynamic library

Date of creation: 2010-15-11

Copyright
Julian David Colorado, jd.colorado@upm.es
*/

#include <iostream>
#include <fstream>
#include <string>
#include <math.h>
#include "newmat/newmatap.h"
#include "newmat/newmatio.h"


/*!
\brief Ordinary differential equations solver with 4 order runge kutta or euler, or eulerimproved

INPUT:
\param MAT SimParam: Simulation Parameters 4 x 6
\param MAT X: State matrix 4 x pt
\param int i: Actual point
\param double u: Control action

OUTPUT:

\param MAT X: i+1 position modified.

\sa euler.cpp improvedeuler.cpp rungekutta.cpp
*/

void odesolver(Matrix &SimParam,Matrix &X,int i,Matrix &u);


/*!
\brief Dynamic Model


INPUT
\param MAT SimParam: Simulation Parameters 4 x 6
\param VEC X: State vector 4
\param double TAU: Torque input

OUTPUT

\param VEC out: State modified.

\sa acrobot.cpp pendubot.cpp cartpole.cpp completefuruta.cpp inertiawheel.cpp
*/

void dynmodel(Matrix &SimParam,Matrix &X,int i,double Tau,ColumnVector &out);

/*!
\brief Main controller

INPUT
\param MAT SimParam: Simulation Parameters 4 x 6
\param VEC X: State vector 4
\param int i: Time flag
\param int ui: Real time

OUTPUT
\param double TAU: Torque input
\param VEC X: State derivate

*/
void controller(Matrix &SimParam,Matrix &Xm,int i,int ui,double *Tau,ColumnVector &dX);

/*!
\brief Secondary controller - Swing up controller

INPUT
\param MAT SimParam: Simulation Parameters 4 x 6
\param VEC X: State vector 4
\param int i: Time flag
\param int ui: Real time

OUTPUT
\param double TAU: Torque input
\param VEC X: State derivate
*/
void swing_up(Matrix &SimParam,Matrix &Xm,int i,int ui,double *Tau,ColumnVector &dX);

/*!
\brief DC motors library

INPUT
\param MAT SimParam: Simulation Parameters 4 x 6
\param double u: Control action
\param int i: Time flag

OUTPUT
\param double X: control action

\sa motor1.cpp motor2.cpp motor3.cpp
*/
double dcmotors(Matrix &SimParam,double u,Matrix &X,int i);


/*!

\brief Ordinary Differential equation function, complete state variables constructor

INPUT
\param MAT SimParam: Simulation Parameters 4 x 6
\param VEC X: State vector 4
\param int i: Time flag
\param MATRIX u: Control action 1 x n matrix.
\param int ui: Real time

OUTPUT
\param MATRIX u: Control action 1 x n matrix. (ui +1 position modified)

*/
void odefunc(Matrix &SimParam,Matrix &X,int i,Matrix &u,int ui,ColumnVector &out);
